#include "kalman_filter/kalman.h"
#include "test_kf.h"
#include <stdio.h>

float measure[] = {
	0.0168, -0.0176, 0.0025, -0.0098, 0.0081, -0.0089,
	0.0143, 0.0209, 0.0422, 0.0062, -0.0303, -0.0017,
	0.0451, -0.0003, 0.0437, 0.0306, 0.0607, -0.0031,
	0.0365, 0.0210 ,0.1082, 0.0716, 0.0881, 0.0450,
	0.0626, 0.0727, 0.1065, 0.0856, 0.1120, 0.0641,
	0.1054, 0.1037, 0.0965, 0.1463, 0.1501, 0.1538,
	0.1353, 0.1937, 0.1875, 0.1841, 0.2005, 0.2049,
	0.1855, 0.2254, 0.2254, 0.2335, 0.2414, 0.2655,
	0.2479, 0.3194, 0.3229, 0.3247, 0.3373, 0.3352,
	0.3849, 0.3755, 0.3777, 0.4332, 0.4160, 0.4233,
	0.4441, 0.4482, 0.4581, 0.5466, 0.5451, 0.5343,
	0.5194, 0.5438, 0.5745, 0.6110, 0.5859, 0.5835,
	0.6190, 0.6728, 0.6923, 0.7122, 0.7194, 0.7448,
	0.7510, 0.7974, 0.7728, 0.8292, 0.8235, 0.8544,
	0.8931, 0.9239, 0.9021, 0.9713, 0.9812, 0.9888,
	1.0086, 1.0308, 1.0519, 1.0816, 1.1055, 1.1446,
	1.1825, 1.1855, 1.1963, 1.2376
};

float state[3] = {0.0f};
float P[9] = {
	0.001f, 0.0f, 0.0f,
	0.0f, 0.001f, 0.0f,
	0.0f, 0.0f, 0.001f
};

float A[9] = {0.0f};
float H[3] = {0.0f};

float Q[9] = {
	0.0002f, 0.0f, 0.0f,
	0.0f, 0.0005f, 0.0f,
	0.0f, 0.0f, 0.0001f
};

float R = 0.004;

void test_process(void *vfilter);
void test_measurement(void *vfilter, float *esti_out);
void test_computeA(void *vfilter);
void test_computeQ(void *vfilter);
void test_computeH(void *vfilter);
void test_computeR(void *vfilter);

struct kalman_filter testFilter = {
	.state_dim = 3,
	.measure_dim = 1,
	.x = state,
	.P = P,
	.A = A,
	.H = H,
	.Q = Q,
	.R = &R,
	.process = test_process,
	.measurement = test_measurement,
	.computeA = test_computeA,
	.computeQ = test_computeQ,
	.computeH = test_computeH,
	.computeR = test_computeR
};

void run_kalman_test(void)
{
	uint16_t total_step = sizeof(measure)/4;
	static uint16_t i = 1;
	float a = 0.15; /* m/s^2 */
	float dt = 0.05; /* sec */

	if (i < total_step) {
		kf_predict(&testFilter, &a, dt);
		kf_update(&testFilter, measure + i);
		printf("%f %f %f\r\n", testFilter.x[0]
				, testFilter.x[1]
				, testFilter.x[2]);
		i++;
	}
}

void test_process(void *vfilter)
{
	struct kalman_filter *filter = (struct kalman_filter *)vfilter;
	float v_k = filter->x[1]
		+ (filter->u[0] - filter->x[2]) * filter->dt;
	filter->x[0] += (v_k + filter->x[1]) * 0.5f * filter->dt;
	filter->x[1] = v_k;
}

void test_measurement(void *vfilter, float *esti_out)
{
	struct kalman_filter *filter = (struct kalman_filter *)vfilter;
	esti_out[0] = filter->x[0];
}

void test_computeA(void *vfilter)
{
	struct kalman_filter *filter = (struct kalman_filter *)vfilter;
	float *pA = filter->A;

	pA[0] = 1.0f;	pA[1] = filter->dt;	pA[2] = 0.0f;
	pA[3] = 0.0f;	pA[4] = 1.0f;		pA[5] = -filter->dt;
	pA[6] = 0.0f;	pA[7] = 0.0f;		pA[8] = 1.0f;
}

void test_computeQ(void *vfilter)
{
	return;
}

void test_computeH(void *vfilter)
{
	struct kalman_filter *filter = (struct kalman_filter *)vfilter;
	float *pH = filter->H;
	pH[0] = 1.0f;	pH[1] = 0.0f;	pH[2] = 0.0f;
}

void test_computeR(void *vfilter)
{
	return;
}
